#!/usr/bin/env python3
# -*- coding:utf-8 -*-
# @Author: renjin@bit.edu.cn
# @Date  : 2024-12-14

"""
【节点名称】：
    SMS2DTargetsToMavlink102
【依赖项安装】：
    pip install spirems
    pip install pymavlink pyserial
【订阅类型】：
    spirecv_msgs::2DTargets
【发布类型】：
    None
【构造参数说明】：
    parameter_file (str): 全局参数文件
【节点参数】：
    resize (list): 图像重采样分辨率
    fps (int): 输出帧率
【备注】：
    无
"""
import os.path
import threading
import time
import cv2
import argparse
from queue import Queue
from typing import Union
from spirems import Publisher, Subscriber, cvimg2sms
from spirecv.base.BaseNode import BaseNode
from pymavlink import mavutil


class SMS2DTargetsToMavlink102Node(threading.Thread, BaseNode):
    def __init__(
        self,
        job_name: str,
        specified_input_topic: str,
        ip: str = '127.0.0.1',
        port: int = 9094,
        param_dict_or_file: Union[dict, str] = None
    ):
        threading.Thread.__init__(self)
        BaseNode.__init__(
            self,
            self.__class__.__name__,
            job_name,
            ip=ip,
            port=port,
            param_dict_or_file=param_dict_or_file
        )
        self.job_queue = Queue()
        self.queue_pool.append(self.job_queue)
        self._results_reader = Subscriber(
            specified_input_topic, 'spirecv_msgs::2DTargets', self.results_callback,
            ip=ip, port=port
        )

        self.serial_port = self.get_param("serial_port", '/dev/ttyTHS0')
        self.baud = self.get_param("baud", 115200)
        print("serial_port:", self.serial_port)
        print("baud:", self.baud)

        self.connection = mavutil.mavlink_connection(self.serial_port, baud=self.baud)
        print("Waiting for heartbeat ...")
        self.connection.wait_heartbeat()
        print("Heartbeat received!")

        self.start()

    def release(self):
        BaseNode.release(self)

    def results_callback(self, msg):
        self.job_queue.put(msg)

    def run(self):
        while self.is_running():
            msg = self.job_queue.get(block=True)
            if msg is None:
                break
            # print(msg)
            if 'fei_position' in msg:
                timestamp_usec = int(msg['timestamp'] * 1e6)
                x, y, z = msg['fei_position'][0], msg['fei_position'][1], msg['fei_position'][2]
                yaw = msg['fei_yaw_a'] / 57.3
                self.connection.mav.vision_position_estimate_send(
                    timestamp_usec, x, y, z, 0, 0, yaw
                )
                print([timestamp_usec, x, y, z, 0, 0, yaw])

        self.release()
        print('{} quit!'.format(self.__class__.__name__))


if __name__ == '__main__':
    parser = argparse.ArgumentParser()
    parser.add_argument(
        'config',
        help='SpireCV2 Config (.json)')
    parser.add_argument(
        '--job-name',
        type=str,
        default='live',
        help='SpireCV Job Name')
    parser.add_argument(
        '--input-topic',
        type=str,
        default='/live/detector/results',
        help='Specified Input Topic')

    args = parser.parse_args()
    print("config:", args.config)
    print("job-name:", args.job_name)

    specified_input_topic = args.input_topic if len(args.input_topic) > 0 else None
    print("input-topic:", specified_input_topic)

    SMS2DTargetsToMavlink102Node(args.job_name, specified_input_topic=specified_input_topic, param_dict_or_file=args.config)
